
#include "main.h"
#include "cmsis_os.h"
#include "motor_protocol.h"
#include "usr_protocol.h"
#include "param_storage.h"
#include "error_code.h"

// 外部变量声明
extern uint8_t uart1_tx_buf[];
extern uint8_t uart2_tx_buf[];
extern uint16_t uart1_tx_len;
extern uint16_t uart2_tx_len;
extern int16_t motor_status_reg_map[2][MAX_MOTOR_REG];



void send_motion(uint8_t motor_group,int16_t reg_value,uint16_t id);

void send_postion_to_all_motor(uint8_t motor_group);
void send_positive_torque(uint8_t motor_group);
void send_negative_torque(uint8_t motor_group);
void send_max_speed(uint8_t motor_group);


void init_control_board(void)
{
    for(int group =0; group<2;group++)
    {
        *control_board_base_addr[group]= 0;
        lip[0] = control_board_param_reg_map[group][BROADCAST_IP_1-CONTROL_BOARD_PARAM_BASE];
        lip[1] = control_board_param_reg_map[group][BROADCAST_IP_2-CONTROL_BOARD_PARAM_BASE];
        lip[2] = control_board_param_reg_map[group][BROADCAST_IP_3-CONTROL_BOARD_PARAM_BASE];
        lip[3] = control_board_param_reg_map[group][BROADCAST_IP_4-CONTROL_BOARD_PARAM_BASE];
    }
        
}


/**
 * @brief 发送完整命令帧到UART线程
 * @param motor_group 电机组 (0:UART1, 1:UART2)
 * @param frame_data 命令帧数据
 * @param length 帧长度
 * @return 处理结果 (1:成功, 0:失败)
 *//**
 * @brief 发送完整命令帧到UART线程
 * @param motor_group 电机组 (0:UART1, 1:UART2)
 * @param frame_data 命令帧数据
 * @param length 帧长度
 * @return 处理结果 (1:成功, 0:失败)
 */uint8_t send_uart_frame(uint8_t motor_group, uint8_t* frame_data, uint16_t length) {
    if (length <= MAX_FRAME_SIZE) {
        // 在栈上创建临时命令结构体
        UartFrameCommand_t* cmd = osPoolAlloc(uartCmdPoolHandle);
        
        if (cmd == NULL) {
            *sys_erro[motor_group] |= SYS_ERR_MEMORY;
            return 0;  // Memory allocation failed
        }
        
        cmd->motor_group = motor_group;
        memcpy(cmd->frame_data, frame_data, length);
        cmd->frame_length = length;
        
        // 发送到队列，FreeRTOS会复制整个结构体数据
        if (osMessagePut(uartFrameQueueHandle, (uint32_t)cmd, 200) == osOK) {
            return 1;  // 成功加入队列
        }
        *sys_erro[motor_group] |= SYS_ERR_QUEUE;
        // 发送失败，释放内存
        osPoolFree(uartCmdPoolHandle, cmd);

    }
    
    return 0;  // 失败
}

uint8_t process_modbus_to_motor(uint16_t reg_addr, int16_t reg_value, uint8_t motor_group) {
    // 使用静态缓冲区
    static uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];
    uint16_t reg_addr_base =0;
    
    // 将reg_value转换为字节数组
    data[0] = (reg_value >> 8) & 0xFF;
    data[1] = reg_value & 0xFF;
    
    reg_addr_base = reg_addr;
    if( reg_addr > 400 && reg_addr < 501)
    {
        reg_addr_base = 401;
    }
    if( reg_addr > 500 && reg_addr < 601)
    {
        reg_addr_base = 501;
    }


    switch (reg_addr_base) {
        case BROADCAST_CONTROL_REG:
          send_motion(motor_group,reg_value,0xff);
            break;
        case BROADCAST_SETPOSITION_CMD:
            send_postion_to_all_motor(motor_group);
            break;
        case BROADCAST_CONTROL_CMD:
            send_motion(motor_group,reg_value,reg_addr - reg_addr_base +1 );
            break;
        case BROADCAST_POSITIVE_TORQUE:
            send_positive_torque(motor_group);
            break;
            
        case BROADCAST_NEGATIVE_TORQUE:
            send_negative_torque(motor_group);
            break;
            
        case BROADCAST_MAX_SPEED:
            send_max_speed(motor_group);
            break;
        case BROADCAST_IP_SAVE:
            Param_Save();
            break;
        default:
            return 0;  // 不支持的寄存器
    }
    
    
    return 0;  // 失败
}

// 在Modbus处理线程中发送命令
void send_modbus_command(int16_t addr, int16_t value,int channel) {
    ModbusCommand_t *cmd  = osPoolAlloc(modbusPoolHandle);

    if (cmd == NULL) {
        *sys_erro[channel] |= SYS_ERR_MEMORY;
        return 0;  // Memory allocation failed
    }

    cmd->channel = channel;
    cmd->reg_addr = addr;
    cmd->reg_value = value;
    
    // 将命令发送到队列，超时等待10ms
    if( osMessagePut(modbusCommandQueueHandle, (uint32_t)cmd, 10) == osOK ) {
        // 命令发送成功
    } else {
        // 命令发送失败
        *sys_erro[channel] |= SYS_ERR_QUEUE;
        osPoolFree(modbusPoolHandle, cmd);
    }
}




void send_postion_to_all_motor(uint8_t motor_group)
{

    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[4];
    int16_t reg_value;
    int16_t speed;  
    int16_t position;
    SlaveStatus slave_status;

    speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    if(speed <= 200)
    {
        
        speed = 200;
    }

    uint16_t max_id = control_board_param_reg_map[motor_group][MAX_ID_ADDR];
    if(max_id > MAX_ID)
        max_id = MAX_ID;
    if(max_id ==0)
        max_id =MAX_ID;

    for(int id =0; id < max_id; id++)
    {
        if(checkFlag(motor_position_flags,id))
        {
            slave_status  = device_online_status_base_addr[motor_group][id];
            if(slave_status == SLAVE_OFFLINE)
            {
                continue;
            }
            update_motor_status(motor_group,id+1,1);
            position = motor_status_reg_map[motor_group][MOTOR_SETPOINT_POSTION_BASE + id];

                
            // 将reg_value转换为字节数组
            data[0] = (speed) & 0xFF;
            data[1] = (speed>>8) & 0xFF;
            data[2] = (position) & 0xFF;
            data[3] = (position>>8) & 0xFF;


            frame_len = build_command_frame(temp_buf, id+1, CMD_WRITE_REG,
                MOTOR_CMD_SPEED_SET, data, 4);

            send_uart_frame(motor_group, temp_buf, frame_len);
            osDelay(5);
        }
    }
}

void send_positive_torque(uint8_t motor_group)
{

    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];
    int16_t reg_value;
        
    reg_value = motor_status_reg_map[motor_group][BROADCAST_POSITIVE_TORQUE];
    // 将reg_value转换为字节数组
    data[0] = reg_value  & 0xFF;
    data[1] = (reg_value>>8) & 0xFF;

    frame_len = build_command_frame(temp_buf, 0xff, CMD_WRITE_REG,
        MOTOR_CMD_CURRENT_UPPER, data, 2);

    send_uart_frame(motor_group, temp_buf, frame_len);
}

void send_negative_torque(uint8_t motor_group)
{

    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];
    int16_t reg_value;
        
    reg_value = motor_status_reg_map[motor_group][BROADCAST_NEGATIVE_TORQUE];
    // 将reg_value转换为字节数组
    data[0] = reg_value & 0xFF;
    data[1] = (reg_value>>8) & 0xFF;

    frame_len = build_command_frame(temp_buf, 0xff, CMD_WRITE_REG,
        MOTOR_CMD_CURRENT_LOWER, data, 2);

    send_uart_frame(motor_group, temp_buf, frame_len);
}

void send_max_speed(uint8_t motor_group)
{

    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];
    int16_t reg_value;
        
    reg_value = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    // 将reg_value转换为字节数组
    data[0] = (reg_value >> 8) & 0xFF;
    data[1] = reg_value & 0xFF;

    frame_len = build_command_frame(temp_buf, 0xff, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 2);

    send_uart_frame(motor_group, temp_buf, frame_len);
}

void send_clear_motor_status(uint8_t motor_group, uint16_t id)
{
  
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];

    data[0] = 1;
    data[1] = 0;
    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_CLEAR_ERROR, data, 2);

    send_uart_frame(motor_group, temp_buf, frame_len);
}
void send_motion_pause(uint8_t motor_group,uint16_t id)
{
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[2];

    data[0] = 1;
    data[1] = 0;
    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_PAUSE, data, 2);

    send_uart_frame(motor_group, temp_buf, frame_len);

    
    if(id ==0xff)
    {
        for(int item =0; item < MAX_ID; item++)
        {
            update_motor_status(motor_group,item+1,0);
        }
    }
    else
    {
        update_motor_status(motor_group,id,0);
    }
}

void send_motion_up_down_limit(uint8_t motor_group)
{
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[4];
    const uint8_t BATCH_SIZE = 20;
    int16_t speed =0;
    int16_t position = 0;
    SlaveStatus slave_status;

    uint16_t max_id = control_board_param_reg_map[motor_group][MAX_ID_ADDR];
    if( max_id >  MAX_ID )
        max_id = MAX_ID;
    if(max_id == 0)
        max_id = MAX_ID;

    data[0] = 1;
    data[1] = 0;

    for(int batch_start =0; batch_start < max_id; batch_start +=BATCH_SIZE )
    {
        for(int offset=0; offset<BATCH_SIZE; offset++)
        {
            int motor_id = batch_start + offset;
            
            slave_status  = device_online_status_base_addr[motor_group][motor_id];
            if(slave_status == SLAVE_OFFLINE)
            {
                continue;
            }
            update_motor_testing_up_down_limit(motor_group,motor_id+1,1);
            frame_len = build_command_frame(temp_buf, motor_id+1, CMD_WRITE_REG,
                MOTOR_CMD_LIMIT_ENABLE, data, 2);
                send_uart_frame(motor_group, temp_buf, frame_len);
            osDelay(10);
        }
        osDelay(1000);

    }
    for(int time_cnt = 0; time_cnt < 100; time_cnt++)
    {
        for(int id =0; id < max_id; id++)
        {
            slave_status  = device_online_status_base_addr[motor_group][id];
            if(slave_status == SLAVE_OFFLINE)
            {
                continue;
            }
            frame_len = build_command_frame(temp_buf, id+1, CMD_READ_REG,
                MOTOR_CMD_STATE_WORD, data, 1);
            send_uart_frame(motor_group, temp_buf, frame_len);

        }
        osDelay(100);
    }
    *control_board_base_addr[motor_group] &= ~(0x03<<1);
    *control_board_base_addr[motor_group]|= (0x01<<2);
    
    // 读取上限位
    
    for(int id =0; id < max_id; id++)
    {
        update_motor_testing_up_down_limit(motor_group,id+1,0);
        slave_status  = device_online_status_base_addr[motor_group][id];
        if(slave_status == SLAVE_OFFLINE)
        {
            continue;
        }
        update_motor_testing_up_down_limit_done(motor_group,id+1,1);
    }

    for(int id =0; id < max_id; id++)
    {
        
        frame_len = build_command_frame(temp_buf, id+1, CMD_READ_REG,
            MOTOR_CMD_LIMIT_UPPER, data, 1);
        send_uart_frame(motor_group, temp_buf, frame_len);
        osDelay(10);
    }
    osDelay(100);
    // 读取下限位
    for(int id =0; id < max_id; id++)
    {
        frame_len = build_command_frame(temp_buf, id+1, CMD_READ_REG,
            MOTOR_CMD_LIMIT_LOWER, data, 1);
        send_uart_frame(motor_group, temp_buf, frame_len);
        osDelay(10);
    }
    

    // 读取上限位
    for(int id =0; id < max_id; id++)
    {

        speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
        position = motor_status_reg_map[motor_group][MOTOR_SETPOINT_POSTION_BASE + id];
        if(speed <= 200)
            speed = 200;
        // 将reg_value转换为字节数组
        data[0] = (speed) & 0xFF;
        data[1] = (speed>>8) & 0xFF;
        data[2] = (position) & 0xFF;
        data[3] = (position>>8) & 0xFF;


        frame_len = build_command_frame(temp_buf, id+1, CMD_WRITE_REG,
            MOTOR_CMD_SPEED_SET, data, 4);

        send_uart_frame(motor_group, temp_buf, frame_len);
        osDelay(10);

    }

}

void send_single_motion_up_down_limit(uint8_t motor_group,uint16_t id)
{
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[4];
    const uint8_t BATCH_SIZE = 20;
    int16_t speed =0;
    int16_t position = 0;
    SlaveStatus slave_status;

    uint16_t max_id = control_board_param_reg_map[motor_group][MAX_ID_ADDR];
    if( max_id >  MAX_ID )
        max_id = MAX_ID;
    if(max_id == 0)
        max_id = MAX_ID;

    data[0] = 1;
    data[1] = 0;


    slave_status  = device_online_status_base_addr[motor_group][id-1];
    if(slave_status == SLAVE_OFFLINE)
    {
        return;
    }

    update_motor_testing_up_down_limit(motor_group,id,1);

    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_LIMIT_ENABLE, data, 2);
        send_uart_frame(motor_group, temp_buf, frame_len);




    for(int time_cnt = 0; time_cnt < 100; time_cnt++)
    {
        for(int id =0; id < max_id; id++)
        {
            slave_status  = device_online_status_base_addr[motor_group][id];
            if(slave_status == SLAVE_OFFLINE)
            {
                continue;
            }
            frame_len = build_command_frame(temp_buf, id+1, CMD_READ_REG,
                MOTOR_CMD_STATE_WORD, data, 1);
            send_uart_frame(motor_group, temp_buf, frame_len);

        }
        osDelay(100);
    }
    // 读取上限位
    *control_board_base_addr[motor_group] &= ~(0x03<<1);
    *control_board_base_addr[motor_group]|= (0x01<<2);
    update_motor_testing_up_down_limit(motor_group,id,0);
    update_motor_testing_up_down_limit_done(motor_group,id,1);


    frame_len = build_command_frame(temp_buf, id, CMD_READ_REG,
        MOTOR_CMD_LIMIT_UPPER, data, 1);
    send_uart_frame(motor_group, temp_buf, frame_len);
    osDelay(10);

    // 读取下限位

    frame_len = build_command_frame(temp_buf, id, CMD_READ_REG,
        MOTOR_CMD_LIMIT_LOWER, data, 1);
    send_uart_frame(motor_group, temp_buf, frame_len);

    osDelay(2000);

    speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    position = motor_status_reg_map[motor_group][MOTOR_SETPOINT_POSTION_BASE + id-1];
    if(speed <= 200)
        speed = 200;
    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;


    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);
    osDelay(10);


}



void send_motion_to_single_up_down_running(uint8_t motor_group,uint16_t id)
{
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[4];
    const uint8_t BATCH_SIZE = 20;
    int16_t speed =0;
    int16_t position =0;
    int16_t actual_position = 0;
    int16_t up_limit = 0;
    int16_t down_limit = 0;


    speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    position = 16384;
    if(speed <= 200)
        speed = 200;
    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;

    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);
    osDelay(10);
    int status = 0;

    for(int timeout=0; timeout < 100; timeout++)
    {
        status = 0;

        if( device_online_status_base_addr[motor_group][id-1] == SLAVE_OFFLINE)
            continue;
        
        up_limit = motor_uplimit_positon_base_addr[motor_group][id-1];
        actual_position = motor_actual_positon_base_addr[motor_group][id-1];
        if(actual_position < (up_limit -500))
        {
            status = 1;
        }

        if(status ==0)
        {
            break;
        }
        else
        {
            osDelay(100);

        }
    }


    osDelay(1000);


    position = 0;

    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;

    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);

    for(int timeout=0; timeout < 100; timeout++)
    {
        status = 0;

        if( device_online_status_base_addr[motor_group][id-1] == SLAVE_OFFLINE)
            continue;
        
        down_limit = motor_downlimit_position_base_addr[motor_group][id-1];
        actual_position = motor_actual_positon_base_addr[motor_group][id-1];
        if(actual_position > (down_limit + 500))
        {
            status = 1;
        }

        if(status ==0)
        {
            break;
        }
        else
        {
            osDelay(100);

        }
    }

    *control_board_base_addr[motor_group] &= ~(0x03<<3);
    *control_board_base_addr[motor_group]|= (0x01<<4);

    speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    position = motor_status_reg_map[motor_group][MOTOR_SETPOINT_POSTION_BASE + id-1];
    if(speed <= 200)
        speed = 200;
    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;


    frame_len = build_command_frame(temp_buf, id, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);
    osDelay(10);



}

void send_all_motion_up_down_running(uint8_t motor_group)
{
    uint8_t temp_buf[MAX_FRAME_SIZE];
    uint16_t frame_len = 0;
    uint8_t data[4];
    const uint8_t BATCH_SIZE = 20;
    int16_t speed =0;
    int16_t position =0;
    int16_t actual_position = 0;
    int16_t up_limit = 0;
    int16_t down_limit = 0;

    uint16_t max_id = control_board_param_reg_map[motor_group][MAX_ID_ADDR];
    if( max_id >  MAX_ID )
        max_id = MAX_ID;
    if(max_id == 0)
        max_id = MAX_ID;


    speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
    position = 16384;
    if(speed <= 1)
        speed = 2048;
    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;


    frame_len = build_command_frame(temp_buf, 0xff, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);

    int status = 0;

    for(int timeout=0; timeout < 100; timeout++)
    {
        status = 0;
        for(int id =0 ; id < max_id; id++ )
        {
            if( device_online_status_base_addr[motor_group][id] == SLAVE_OFFLINE)
                continue;
            
            up_limit = motor_uplimit_positon_base_addr[motor_group][id];
            actual_position = motor_actual_positon_base_addr[motor_group][id];
            if(actual_position < (up_limit -500))
            {
                status = 1;
            }
        }
        if(status ==0)
        {
            break;
        }
        else
        {
            osDelay(100);

        }
    }


    osDelay(1000);


    position = 0;

    // 将reg_value转换为字节数组
    data[0] = (speed) & 0xFF;
    data[1] = (speed>>8) & 0xFF;
    data[2] = (position) & 0xFF;
    data[3] = (position>>8) & 0xFF;

    frame_len = build_command_frame(temp_buf, 0xff, CMD_WRITE_REG,
        MOTOR_CMD_SPEED_SET, data, 4);

    send_uart_frame(motor_group, temp_buf, frame_len);

    for(int timeout=0; timeout < 100; timeout++)
    {
        status = 0;
        for(int id =0 ; id < max_id; id++ )
        {
            if( device_online_status_base_addr[motor_group][id] == SLAVE_OFFLINE)
                continue;
            
            down_limit = motor_downlimit_position_base_addr[motor_group][id];
            actual_position = motor_actual_positon_base_addr[motor_group][id];
            if(actual_position > (down_limit + 500))
            {
                status = 1;
            }
        }
        if(status ==0)
        {
            break;
        }
        else
        {
            osDelay(100);

        }
    }

    *control_board_base_addr[motor_group] &= ~(0x03<<3);
    *control_board_base_addr[motor_group]|= (0x01<<4);

    // 读取上限位
    for(int id =0; id < max_id; id++)
    {

        speed = control_board_param_reg_map[motor_group][BROADCAST_MAX_SPEED-2000];
        position = motor_status_reg_map[motor_group][MOTOR_SETPOINT_POSTION_BASE + id];
        if(speed <= 200)
            speed = 200;
        // 将reg_value转换为字节数组
        data[0] = (speed) & 0xFF;
        data[1] = (speed>>8) & 0xFF;
        data[2] = (position) & 0xFF;
        data[3] = (position>>8) & 0xFF;


        frame_len = build_command_frame(temp_buf, id+1, CMD_WRITE_REG,
            MOTOR_CMD_SPEED_SET, data, 4);

        send_uart_frame(motor_group, temp_buf, frame_len);
        osDelay(10);

    }
    
}

void send_motion(uint8_t motor_group,int16_t reg_value,uint16_t id)
{
    uint8_t cmd = reg_value;

    switch (cmd)
    {
    case 0x01:
        send_clear_motor_status(motor_group,id);
        break;
    case 0x02:
        send_motion_pause(motor_group,id);
        /* code */
        break;
    case 0x04:
        *control_board_base_addr[motor_group]= (0x01<<1);
        if(id == 0xff)
        {
            send_motion_up_down_limit(motor_group);
        }
        else
        {
            send_single_motion_up_down_limit(motor_group,id);
        }
        /* code */
        break;
    case 0x08:
        *control_board_base_addr[motor_group] &= ~(0x03<<3);
        *control_board_base_addr[motor_group]|= (0x01<<3);
        if(id ==0xff)
        {
            
            send_all_motion_up_down_running(motor_group);
        }
        else
        {
            send_motion_to_single_up_down_running(motor_group,id);
        }
        /* code */
        break;
    
    default:
        break;
    }
}



/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void ControlTask(void const * argument)
{
  /* USER CODE BEGIN 5 */
  /* Infinite loop */
  for(;;)
  {
    
    osEvent event = osMessageGet(modbusCommandQueueHandle, 1000); // 阻塞等待1秒

    if(event.status == osEventMessage)
    {
        ModbusCommand_t* cmd = (ModbusCommand_t*)event.value.p;
        if( cmd != NULL )
        {
            uint16_t channel = cmd->channel;
            int16_t modbus_reg_addr = cmd->reg_addr;
            int16_t modbus_reg_value = cmd->reg_value;
            process_modbus_to_motor(modbus_reg_addr,modbus_reg_value,channel);
            osPoolFree(modbusPoolHandle, cmd); // 释放内存
        }
    }
    else
    {
 

    }
    

    osDelay(10);
}

  /* USER CODE END 5 */
}

